/******************************************************************************
 * Copyright 2019 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

/**
 * @file
 * Modified function input and used only some functions
 **/
#pragma once
#include "FrenetPath.h"
#include "CubicSpline2D.h"
#include "vec2d.h"
#include "path_points.h"
#include "reference_line.h"
#include <ros/ros.h>

class PathData
{
public:
  enum class PathPointType
  {
    IN_LANE,
    OUT_ON_FORWARD_LANE,
    OUT_ON_REVERSE_LANE,
    OFF_ROAD,
    UNKNOWN,
  };
  PathData() = default;
  // PathData(const ReferenceLine &reference_line);
  ~PathData() = default;
  bool SetDiscretizedPath(DiscretizedPath path);

  bool SetFrenetPath(FrenetFramePath frenet_path);

  bool SetPathPointDecisionGuide(std::vector<std::tuple<double, PathPointType, double>> path_point_decision_guide);

  void SetReferenceLine(const ReferenceLine *reference_line);

  const DiscretizedPath &discretized_path() const;

  const FrenetFramePath &frenet_frame_path() const;

  const std::vector<std::tuple<double, PathPointType, double>> &path_point_decision_guide() const;

  PathPoint GetPathPointWithPathS(const double s) const;
  FrenetFramePoint GetFrenetFramePointWithPathS(const double s) const;

  /*
   * brief: this function will find the path_point in discretized_path whose
   * projection to reference line has s value closest to ref_s.
   */
  bool GetPathPointWithRefS(const double ref_s, PathPoint *const path_point) const;

  std::list<std::pair<DiscretizedPath, FrenetFramePath>> &path_data_history();

  bool LeftTrimWithRefS(const FrenetFramePoint &frenet_point);

  void Clear();

  bool Empty() const;

  int Size() const;

  double Length() const;

  std::string DebugString() const;

  void set_path_label(const std::string &label);

  const std::string &path_label() const;

  void set_blocking_obstacle_id(const std::string &obs_id)
  {
    blocking_obstacle_id_ = obs_id;
  }
  const std::string &blocking_obstacle_id() const
  {
    return blocking_obstacle_id_;
  }

  const bool is_valid_path_reference() const
  {
    return is_valid_path_reference_;
  }
  void set_is_valid_path_reference(bool is_valid_path_reference)
  {
    is_valid_path_reference_ = is_valid_path_reference;
  }

  const bool is_optimized_towards_trajectory_reference() const
  {
    return is_optimized_towards_trajectory_reference_;
  }
  void set_is_optimized_towards_trajectory_reference(bool is_optimized_towards_trajectory_reference)
  {
    is_optimized_towards_trajectory_reference_ = is_optimized_towards_trajectory_reference;
  }

  const std::vector<PathPoint> &path_reference() const;
  void set_path_reference(const std::vector<PathPoint> &path_reference);

private:
  /*
   * convert frenet path to cartesian path by reference line
   */
  bool SLToXY(const FrenetFramePath &frenet_path, DiscretizedPath *const discretized_path);
  bool XYToSL(const DiscretizedPath &discretized_path, FrenetFramePath *const frenet_path);

  /**
   * @brief speed decision generated by path analyzer for guiding speed limit
   * generation in speed bounds decider
   * @param tuple consists of s axis position on reference line; PathPointType
   * Enum; distance to closest obstacle
   */
  std::vector<std::tuple<double, PathPointType, double>> path_point_decision_guide_;
  std::string path_label_ = "";
  std::string blocking_obstacle_id_;

  /**
   * @brief parameters for using the learning model output as a path reference
   *
   */
  // wheter this PathData is a path reference serving as an optimization target
  // for later modules
  bool is_valid_path_reference_ = false;

  /**
   * @brief Given a trajectory reference, whether this PathData is optimized
   * according to the "path" part of the trajectory so that "speed" part of the
   * trajectory could be used in later modules accordingly
   *
   */
  bool is_optimized_towards_trajectory_reference_ = false;

  // path reference
  std::vector<PathPoint> path_reference_;
  // ReferenceLine reference_line_;
  const ReferenceLine *reference_line_ = nullptr;
  DiscretizedPath discretized_path_;
  FrenetFramePath frenet_path_;
  std::list<std::pair<DiscretizedPath, FrenetFramePath>> path_data_history_;
};
